#include <iostream>

using namespace std;

////////////////////////  MATERIAL ////////////////////

class Material {
public:
	int iId;
	double dYoungsModulus;
	double dPoissonsRatio;

	Material(int iId, double dYoungsModulus, double dPoissonsRatio) {
		this->iId = iId;
		this->dYoungsModulus = dYoungsModulus;
		this->dPoissonsRatio = dPoissonsRatio;
	}
};

////////////////////// NODE ////////////////////////

class Node {
public:
	int iId;
	double iX;
	double iY;
	double iZ;

public:
	Node(int iId, double iX, double iY, double iZ) {
		this->iId = iId;
		this->iX = iX;
		this->iY = iY;
		this->iZ = iZ;
		cout << "Node created" << endl;
	}

	Node() {
		this->iId = 0;
		this->iX = 0;
		this->iY = 0;
		this->iZ = 0;
		cout << "Empty node created" << endl;
	}
};

///////////////// PROPERTY //////////////////////

class Property {
public:
	int iId;
	Material *mMaterial;
	int iProp1;
	double dReal1;

	Property(Material *myMaterial, int iId, int iProp1 = 0, double dReal1 = 0) {
		this->iId = iId;
		this->mMaterial = myMaterial;
		this->iProp1 = iProp1;
		this->dReal1 = dReal1;
	}
};

///////////////// QUAD4 /////////////////////////

class Quad4 {
private:
	//Variables
	int iId;
	int iType; //0=Axissymmetric, 1=Plane Strain, 2=Plane Stress
	int iIntegrationOrder = 2;

	double dThickness;
	double dJacobian;
	double aNodes[2][4];
	double aStressStrainMatrix[3][3];
	double aStrainDisplacementMatrix[3][8];
	double aStiffnessMartix[8][8];

	Property *pProperty;

	void createStressStrainMatrix() {
		if (iType == 2) {
			double dNue = pProperty->mMaterial->dPoissonsRatio;
			double dBeforeBracket = pProperty->mMaterial->dYoungsModulus / (1 - dNue * dNue);

			aStressStrainMatrix[0][0] = dBeforeBracket;
			aStressStrainMatrix[0][1] = dBeforeBracket * dNue;
			aStressStrainMatrix[1][1] = dBeforeBracket;
			aStressStrainMatrix[2][2] = dBeforeBracket * ((1 - dNue) / 2);
			aStressStrainMatrix[0][2] = 0;
			aStressStrainMatrix[1][2] = 0;
			aStressStrainMatrix[1][0] = aStressStrainMatrix[0][1];
			aStressStrainMatrix[2][0] = 0;
			aStressStrainMatrix[2][1] = 0;

			///// DEBUG
			for (int i = 0; i < 3; i++) {
				for (int j = 0; j < 3; j++) {
					cout << aStressStrainMatrix[i][j] << "\t";
				}
				cout << endl;
			}

			cout << endl;
		}
	}

	void calculateStrainDisplacementMatrix(double dR, double dS) {
		double dJacobian;

		double aShapeFunction[4];
		double aShapeFunctionDerivatives[2][4];
		double aJacobiMatrix[2][2];
		double aInverseJacobiMatrix[2][2];

		aShapeFunction[0] = .25 * (1 + dR) * (1 + dS);
		aShapeFunction[1] = .25 * (1 - dR) * (1 + dS);
		aShapeFunction[2] = .25 * (1 - dR) * (1 - dS);
		aShapeFunction[3] = .25 * (1 + dR) * (1 - dS);

		//Derivatives of shape functions
		//Derive by r
		aShapeFunctionDerivatives[0][0] = .25 * (1 + dS);
		aShapeFunctionDerivatives[0][1] = -1 * aShapeFunctionDerivatives[0][0];
		aShapeFunctionDerivatives[0][2] = -.25 * (1 - dS);
		aShapeFunctionDerivatives[0][3] = -1 * aShapeFunctionDerivatives[0][2];
		//Derive by s
		aShapeFunctionDerivatives[1][0] = .25 * (1 + dR);
		aShapeFunctionDerivatives[1][1] = .25 * (1 - dR);
		aShapeFunctionDerivatives[1][2] = -1 * aShapeFunctionDerivatives[1][1];
		aShapeFunctionDerivatives[1][3] = -1 * aShapeFunctionDerivatives[1][0];

		//Jacobi matrix at point r|s
		for (int i = 0; i < 2; i++) {
			for (int j = 0; j < 2; j++) {
				double dTemp = 0;
				for (int k = 0; k < 4; k++) {
					dTemp += aShapeFunctionDerivatives[i][k] * aNodes[j][k];
				}
				aJacobiMatrix[i][j] = dTemp;
			}
		}

		//Calculate Jacobian
		dJacobian = aJacobiMatrix[0][0] * aJacobiMatrix[1][1] - aJacobiMatrix[0][1] * aJacobiMatrix[1][0];
		if (dJacobian < 0.00000001) {
			cout << "ERROR! Jacobian of element " << iId << " equal or smaller 0!" << endl;
		}

		//Calculate inverse of Jacobi matrix
		aInverseJacobiMatrix[0][0] = 1 / dJacobian * aJacobiMatrix[1][1];
		aInverseJacobiMatrix[0][1] = -1 / dJacobian * aJacobiMatrix[1][0];
		aInverseJacobiMatrix[1][0] = -1 / dJacobian * aJacobiMatrix[0][1];
		aInverseJacobiMatrix[1][1] = 1 / dJacobian * aJacobiMatrix[0][0];

		//Calculate Strain Dispacement matrix
		int k2 = -1;
		for (int k = 0; k < 4; k++) {
			k2 += 2;
			aStrainDisplacementMatrix[0][k2 - 1] = 0;
			aStrainDisplacementMatrix[0][k2] = 0;
			aStrainDisplacementMatrix[1][k2 - 1] = 0;
			aStrainDisplacementMatrix[1][k2] = 0;
			for (int i = 0; i < 2; i++) {
				aStrainDisplacementMatrix[0][k2 - 1] += aInverseJacobiMatrix[0][i] * aShapeFunctionDerivatives[i][k];
				aStrainDisplacementMatrix[1][k2] += aInverseJacobiMatrix[1][i] * aShapeFunctionDerivatives[i][k];
			}
			aStrainDisplacementMatrix[2][k2] = aStrainDisplacementMatrix[0][k2 - 1];
			aStrainDisplacementMatrix[2][k2 - 1] = aStrainDisplacementMatrix[1][k2];
		}

		///DEBUG
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 8; j++) {
				cout << aStrainDisplacementMatrix[i][j] << "\t";
			}
			cout << endl;
		}
	}

	void createStiffnessMatrix() {
		//Clear array
		for (int i = 0; i < 8; i++) {
			for (int j = 0; j < 8; j++) {
				double aStiffnessMartix[i][j] = 0;
			}
		}


	}

public:
	Quad4(int iId, Property *pProperty, Node *nN1, Node *nN2, Node *nN3, Node *nN4) {
		this->iId = iId;
		this->pProperty = pProperty;
		aNodes[0][0] = nN1->iX;
		aNodes[1][0] = nN1->iY;
		aNodes[0][1] = nN2->iX;
		aNodes[1][1] = nN2->iY;
		aNodes[0][2] = nN3->iX;
		aNodes[1][2] = nN3->iY;
		aNodes[0][3] = nN4->iX;
		aNodes[1][3] = nN4->iY;

		dThickness = pProperty->dReal1;
		iType = pProperty->iProp1;

		cout << "QUAD4 Element initialized" << endl;

		createStressStrainMatrix();
		calculateStrainDisplacementMatrix(0, 0);
	}

};

///////////// MAIN  ////////////////////////////
int main() {
	//Create Material
	Material material1(1, 200000., .3);
	//Create Property
	Property prop1(&material1, 1, 2);
	//Create nodes
	Node node1(1, 1, 1.25, 0);
	Node node2(2, -1, .25, 0);
	Node node3(3, -1, -.75, 0);
	Node node4(4, 1, -.75, 0);
	//Create elements
	Quad4(1, &prop1, &node1, &node2, &node3, &node4);

	return 0;
}
